/*
 * @Date: 2020-10-19 22:22:36
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-25 16:30:31
 * @FilePath: /swarm_ws/src/px4_gcs/src/HGJMultiUAVCoverage.cpp
 * @Description: HGJ coverage
 */
//
// Created by sun on 2020/10/19.
//
#include "OffboardCore.h"
#include <boost/bind.hpp>
#include "ros/ros.h"
#include "Utils/TrajManager.h"
#include "Utils/CSVManager.h"
#include "Utils/RosUtils.h"

int main(int argc, char **argv) {
    //>>>>>>>>>>>>>>>init<<<<<<<<<<<<<<<
    ros::init(argc, argv, "HGJ_UAV_COVERAGE");
    ros::NodeHandle nh;

    //>>>>>>>>>>>>>>>load traj<<<<<<<<<<<<<<<
    ROS_INFO("Now loading trajectory");
    std::string path;
    path = RosUtils::readParam<std::string>(nh,"readfilepath","/traj/trj2_recv");
    path = ros::package::getPath("px4_gcs") + path + ".csv";
    CSVManager<float> csvManager(path);
    vector<vector<float>> trajectory;
    while(!csvManager.readCSV(trajectory)){
        sleep(1);
    }
    csvManager.readCSV(trajectory);
    if(trajectory[0].size()==5&&trajectory[0][2]==trajectory.size()){
        ROS_INFO_STREAM("loaded trajectory successful !! size: "<< trajectory.size() << " X " << trajectory[0].size());
    }else if(trajectory[0].size()==2){
        ROS_INFO_STREAM("loaded trajectory successful !! size: "<< trajectory.size() << " X " << trajectory[0].size());
    }

    //>>>>>>>>>>>>>>>traj process<<<<<<<<<<<<<<<
    ROS_INFO("Now process trajectory");
    TrajManager<float> trajManager;
    float height = 6.0f;
    height = RosUtils::readParam(nh,"takeoff_height",height);
    for (auto & j : trajectory) {
        vector<float> curTraj(3);
        // 坐标系转换
        // x = traj_y , y = -traj_x
        curTraj[1] = -j[trajectory[0].size()-2];
        curTraj[0] = j[trajectory[0].size()-1];
        curTraj[2] = height;
        trajManager.traj.push_back(curTraj);
    }

    //>>>>>>>>>>>>>>>init offboard control<<<<<<<<<<<<<<<
    ROS_INFO("Init offboard control");
    OffboardCore control(nh);
    ros::spinOnce();
    control.initRosSubPubManager(nh);
    control.loadTrajFromVector(trajManager.traj);

    //>>>>>>>>>>>>>>>>start<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    ROS_INFO("Now start MainLoop!!!!!!");
    control.mainLoop();
}
